% Pololu Servo Controller - Create object to carry out functions of servo
% controller
%
% This script assumes one servo controller for one serial port
%
% Make sure servo controller is in "Dual USB Mode"
%
% Make sure to specify the "Command Port" of the serial ports
%
% Written by Eric Lam, April 2013
classdef ServoController
    properties(Constant)
        PAUSE_TIME = 0.060; % Wait time during response of servo controller
    end
    
    properties(GetAccess = 'public', SetAccess = 'private')
        comPortNumber; % Serial Port Number
        port; % Serial Port Object
    end
    
    methods
        % Constructor, intialize servo controller
        function obj = ServoController(comPortNumber)
            %delete(instrfind); % Free/delete all used COM ports
            obj.comPortNumber = comPortNumber;
            obj.port = serial(['COM', num2str(comPortNumber)]);
            fopen(obj.port);
            set(obj.port, 'Timeout', 1); %Set timeout of fread
        end
        
        % Move servo to specified target
        % obj - servoController object
        % channel - Channel of the servo controller wishing to be controlled
        % target - Number ranging from 0 to 254, 127 being the neutral
        % position
        function setPosition(obj, channel, target)
            fwrite(obj.port, [255 channel target], 'uint8');
        end
        
        % Sets speed limit of a servo
        % Default is __
        % channel - channel of servo
        % speed - value in units of (0.25 microseconds)/(10 ms)
        function setSpeed(obj, channel, speed)
            speedInBinary = dec2bin(speed);
            speedInBinaryLength = length(speedInBinary);
            
            % Divide speed channel into two groups of 7 bits
            if (speedInBinaryLength > 7 && speedInBinaryLength <= 14)
                highBits = bitshift(speed, -7); % Shift speed byte by 7 bits to right
                lowBits = speed - highBits*2^(7); % Calculate lower 7 bits
            elseif (speedInBinaryLength <= 7) % Speed bits is able to fit into 1 7-bit group
                lowBits = speedInBinary;
                highBits = 0;
            else
                fprintf('Error with given speed value. May be too high or negative. Max supported bits is 14'); 
            end           
            
            fwrite(obj.port, [135 channel lowBits highBits]);
        end
        
        % Sets acceleration limit of a servo
        % Causes servo to accelerate until at max speed, and then
        % deaccelerate as it reaches its target position
        % Default is __
        % channel - channel of servo
        % acceleration - value in units of (0.25 microseconds)/(10 ms)/(80 ms). Limited to values between 0 to
        % 255
        function setAcceleration(obj, channel, acceleration)
            accelInBinary = dec2bin(acceleration);
            accelInBinaryLength = length(accelInBinary);
            
            % Divide speed channel into two groups of 7 bits
            if (accelInBinaryLength > 7 && accelInBinaryLength <= 14)
                highBits = bitshift(acceleration, -7); % Shift speed byte by 7 bits to right
                lowBits = speed - highBits*2^(7); % Calculate lower 7 bits
            elseif (length(accelInBinary) <= 7) % Speed bits is able to fit into 1 7-bit group
                lowBits = accelInBinary;
                highBits = 0;
            else
                fprintf('Error with given acceleration value. May be too high or negative. Max supported bits is 14'); 
            end           
            
            fwrite(obj.port, [135 channel lowBits highBits]);
        end
        
        % Implementation of Get Position. Response is two bytes.
        % If configured to a: 
        % Servo: Position represents PWM output to the channel
        % Input : Position represents voltage input
        function position = getPosition(obj, channel)
            fwrite(obj.port, [144 channel]);
            pause(obj.PAUSE_TIME); % Let the servo controller respond
            response = fread(obj.port, 2);
          
            if (length(response) == 2)
                position = response(1) + response(2)*(2^7);
            else
                fprintf('Error: Not enough response bytes collected.');
                position = 0; %Indicates error
            end
        end
        
        % Obtains servo position of a certain channel
        % @return servoPosition value representing servo position from _
        % microseconds to _ microseconds
        function servoPosition = getServoPosition(obj, channel)
            servoPosition = obj.getPosition(channel)/4;
        end
        
        % Obtains analog input of a certain channel
        % @return analogInput value from 0 to 1024 representing the voltage
        % being inputted to the channel. 0 meaning 0V and 1023 meaning 5V
        function analogInput = getAnalogInput(obj, channel)
            analogInput = obj.getPosition(channel);
        end
        
        % Returns 1 if any servo is still trying to move
        % Returns 0 if all servos have reached their target position
        % Returns -1 if there is an error
        function movingState = getMovingState(obj)
            fwrite(obj.port, 147);
            pause(obj.PAUSE_TIME);
            response = fread(obj.port, 1);
            
            if (length(response) == 1)
                movingState = response;
            else
                fprintf('Error: Not enough response bytes collected.');
                movingState = -1; %Indicates error
            end 
        end
        
        % Move servos to home position
        function goHome(obj)
           fwrite(obj.port, 162, 'uint8'); 
        end
       
        % Get errors found by the servo controller
        % Returns a error vector where a 1 in the vector corresponds to a
        % certain error indicated by the Pololu Maestro Servo Controller
        % User's Guide
        function errors = getError(obj)
            fwrite(obj.port, 161);
            pause(obj.PAUSE_TIME);
            response = fread(obj.port, 2);
            
            lowBits = de2bi(response(1)); % convert byte response to a binary vector
            highBits = de2bi(response(2)); % convert byte response to a binary vector
            
            errors = [highBits lowBits];
        end
        
        function shutDown(obj)
            fclose(port);
        end
    end
end


%TODO - Make script more user friendly
% e. g. User puts in wrong COM port number...
% - Include failsafes for failed bytes transferred and received like the C
%     code example MAYBE PUT IN ISEMPTY METHOD INSTEAD OF JUST CHECKING
%     LENGTH
% - Test all functions (especially all newly implemented ones)
% - Using speed limits and acceleration limits can give turtle natural
%   instead of sudden ones.
% - WHAT IS THE BIT THAT IS FILLED INTO THE HIGH BYTE OF THE ERROR
%   RESPONSE?
%   - Problem with errors output in getError: vector isn't 9 bits long